clc;clear;close all;
addpath('autogen\');

alpha = [pi/2, -pi/2, pi/2, pi/2, pi/2]';
q = [2.93, -1.57, 0.0, -3.14, 1.57]';


% tau_1s_est = [tau_1s_est; -g'*u11*m1_r1-g'*u21*m2_r2-g'*u31*m3_r3-g'*u41*m4_r4-g'*u51*m5_r5];
u11 = J_T01_q1(q, alpha);
% [-0.515501371821464,0.856888753368947,0,0;-5.24693034519324e-17,-3.15653552478613e-17,0,0;-0.856888753368947,-0.515501371821464,0,0;0,0,0,0]
u21 = J_T02_q1(q, alpha);
% [0.336954183237541,0.390132724529144,0.856888753368947,0;3.42962254925127e-17,3.97088997794913e-17,-3.15653552478613e-17,0;0.560099867429385,0.648495546750919,-0.515501371821464,0;0,0,0,0]
u31 = J_T03_q1(q, alpha);
% [-0.0304381808477086,-0.920255385421976,-0.390132724529144,-0.104502156499933;-4.28886898231504e-17,1.82527537706109e-17,-3.97088997794913e-17,-1.06365485341042e-17;-0.700425459046824,0.298090090682787,-0.648495546750919,-0.173708020002335;0,0,0,0]
u41 = J_T04_q1(q, alpha);
% [0.0193447552734306,0.390839872640013,0.920255385421976,-0.104502156499933;4.17435131239564e-17,4.09110687774613e-17,-1.82527537706110e-17,-1.06365485341042e-17;0.681723304270584,0.668128456399759,-0.298090090682787,-0.173708020002335;0,0,0,0]
u51 = J_T05_q1(q, alpha);
% [0.920270498383626,-0.0186119251957169,-0.390839872640013,-0.153943400388895;-1.82195065087433e-17,-4.17580350437892e-17,-4.09110687774613e-17,-1.58117987344531e-17;-0.297547121691388,-0.681960465219242,-0.668128456399759,-0.258226269736905;0,0,0,0]

g = [0 0 -9.8 0]';

% CAD评估的结果
m5_r5 = 0.08*[0, 0, -0.02, 1]';
m4_r4 = 0.31*[0, -0.10, 0, 1]';
m3_r3 = 0.55*[0.0, 0.0, 0.01, 1]';
m2_r2 = 0.83*[0.01, -0.21, 0.0, 1]';
m1_r1 = 1.30*[-0.12, 0, 0.06, 0]';

tau_1s_est = -g'*u11*m1_r1-g'*u21*m2_r2-g'*u31*m3_r3-g'*u41*m4_r4-g'*u51*m5_r5; %-1.6461

% tau_2s_est = [tau_2s_est; -g'*u22*m2_r2-g'*u32*m3_r3-g'*u42*m4_r4-g'*u52*m5_r5];
% u22 = J_T02_q2(q, alpha);
% [0.648495546750919,-0.560099867429385,0,0;-0.653643620863612,-0.756802495307928,0,0;-0.390132724529144,0.336954183237541,0,0;0,0,0,0]
% u32 = J_T03_q2(q, alpha);
% [-0.611026991972852,-0.217238323590433,0.560099867429385,0.161352568811734;0.615877622413249,0.218962867414309,0.756802495307928,0.202719453744006;0.367591768877291,0.130689839705301,-0.336954183237541,-0.0970691589104716;0,0,0,0]
% u42 = J_T04_q2(q, alpha);
% [0.626689277168564,-0.542518567696766,0.217238323590433,0.161352568811734;-0.594133280219984,-0.773990250603162,-0.218962867414309,0.202719453744006;-0.377014146604277,0.326377332864681,-0.130689839705301,-0.0970691589104716;0,0,0,0]
% u52 = J_T05_q2(q, alpha);
% [0.217737304121815,-0.626516085785674,0.542518567696766,0.229981167625375;-0.219435922188929,0.593958725859254,0.773990250603162,0.300629220445306;-0.130990024702979,0.376909955254914,-0.326377332864681,-0.138355891517854;0,0,0,0]